#!/usr/bin/env python
import rospy
# from sensor_msgs.point_cloud2 import create_cloud
from sensor_msgs.msg import PointCloud2, CameraInfo, NavSatFix
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Twist,TwistStamped,PoseStamped
from sensor_msgs.msg import Image
from nav_msgs.msg import Odometry
import math
# ==============================================================================
# -- main() --------------------------------------------------------------------
# ==============================================================================
def PclCallback(msg, pub):
    msg.header.frame_id = 'velodyne'
    pub.publish(msg)

def ImuCallback(msg, pub):
    msg.header.frame_id = 'imu'
    pub.publish(msg)

def OdomCallback(msg, pub):
    msg.child_frame_id = 'base_link'
    pub.publish(msg)

def RgbCallback(msg, pub):
    msg.header.frame_id = 'camera'
    pub.publish(msg)

def CameraInfoCallback(msg, pub):
    msg.header.frame_id = 'camera'
    pub.publish(msg)

def GnssCallback(msg, pub):
    msg.header.frame_id = 'gnss'
    pub.publish(msg)

def TwistCallback(msg, pub):
    # msg.header.frame_id = 'base_link'
    new_msg = Twist()
    msg.twist.angular.z = -msg.twist.angular.z*3
    if abs(msg.twist.angular.z) >10:
        new_msg=0.0
    new_msg = msg.twist
    pub.publish(new_msg)

def remaper():
    """
    main function
    """
    rospy.init_node('remaper', anonymous=True)

    odom_pub = rospy.Publisher('vehicle/odom',Odometry,queue_size=10)
    imu_pub = rospy.Publisher('imu_raw',Imu,queue_size=10)
    
    rospy.Subscriber("/carla/ego_vehicle/imu/imu1", Imu, ImuCallback,callback_args=imu_pub)
    rospy.Subscriber("/carla/ego_vehicle/odometry", Odometry, OdomCallback,callback_args=odom_pub)

    pcl_pub = rospy.Publisher('points_raw',PointCloud2,queue_size=10)
    rospy.Subscriber("/carla/ego_vehicle/lidar/lidar1/point_cloud", PointCloud2, PclCallback, callback_args=pcl_pub)

    rgb_pub = rospy.Publisher('image_raw',Image,queue_size=10)
    rospy.Subscriber("/carla/ego_vehicle/camera/rgb/front/image_color", Image, RgbCallback, callback_args=rgb_pub)

    camera_info_pub = rospy.Publisher('camera_info',CameraInfo,queue_size=10)
    rospy.Subscriber("/carla/ego_vehicle/camera/rgb/front/camera_info", CameraInfo, CameraInfoCallback, callback_args=camera_info_pub)

    gnss_pub = rospy.Publisher('gnss',NavSatFix,queue_size=10)
    rospy.Subscriber("/carla/ego_vehicle/gnss/gnss1/fix", NavSatFix, GnssCallback, callback_args=gnss_pub)

    twist_pub = rospy.Publisher('/carla/ego_vehicle/twist_cmd',Twist,queue_size=100)
    # rospy.Subscriber("/carla/ego_vehicle/twist_cmd", Twist, TwistCallback, callback_args=twist_pub)
    rospy.Subscriber("/twist_cmd", TwistStamped, TwistCallback, callback_args=twist_pub)
    
    
    # goal_pub = rospy.Publisher('/move_base_simple/goal',PoseStamped, queue_size=10)
    # goal = PoseStamped()
    # goal.header.stamp = rospy.get_rostime()
    # goal.header.frame_id = 'map'
    # goal.pose = 0.0
    # goal.pose.position.x = -10.18
    # goal.pose.position.y = -127.79
    # goal.pose.orientation.z = 1
    # goal.pose.orientation.w = -0.069
    # goal_pub.pub(goal)

    rospy.spin()

if __name__ == '__main__':
    try:
        remaper()
    except rospy.ROSInterruptException:
        pass
